/** 
 * @file Gyro.c
 * 
 * @detail Description : 
 * Gyroscope function calls 
 * 
 * @author E. Macias / D. Torres / S. Ravindran
 * @author Texas Instruments, Inc
 * @date December, 2011
 * @version 1.0 - Initial version
 * @note Built with IAR Embedded Workbench: 5.20.1 and CCS Version 5.1.0.09000
 **/

/* ******************************************************* */
/* I2C code for ITG-3200 Gyro                              */
/*                                                         */
/* ******************************************************* */

#include "Gyro.h"

/**
* @brief <b>Function Name</b>:     : Init_Gyro                                               
* @brief  <b>Description</b>: This function initializes the gyroscope, by 
* setting rate divider, the low pass filter to 256 Hz, and the PLL
* as the clock reference.
* @param Input Parameters: None
* @return Return Values: None
**/ 
void Init_Gyro(void)
{
  // Gyro
  I2C_Set_Slave_Address(ITG3200_ADDR);
  
  //Set the sample rate divider
  global_tx_buffer[0] = 0x15;	// Register
  global_tx_buffer[1] = 0x00; 	
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  Sleep(20);
  
  //Set the DLPF
  global_tx_buffer[0] = 0x16;	// Register
  // Set the full-scale range to +/- 2000deg/sec, and the low pass filter to 256Hz
  global_tx_buffer[1] = (0x03<<3) | (0x00<<0); 	
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  Sleep(20);	
  
  //Setup the clock reference
  global_tx_buffer[0] = 0x3E;	// Power and clock register
  global_tx_buffer[1] = 0x01; 	// Use the PLL with the X Gyro as the clock reference
  I2C_Write_Packet_To_Sensor(&global_tx_buffer[0],2);
  Sleep(20);
  
}

/**
* @brief <b>Function Name</b>:     : Read_Gyro                                               
* @brief  <b>Description</b>: This function reads the gyroscope's x, y, and 
* z value, stores it in the sen_data structure. Afterwards it divides the raw 
* values by the Gyro_Gain values specified by the datasheet, then subtracts the 
* offset from the previus result, and changes the sign for the necessary axes.
* @param Input Parameters: None
* @return Return Values: None
**/ 
void Read_Gyro()
{
  uint8_t rx_temp_buffer[8];  //6 bytes of angular rate data, and 2 bytes of temperature data

  // Gyro
  I2C_Set_Slave_Address(ITG3200_ADDR);
  
  rx_temp_buffer[0] = 0x1B;		//The temperature and gyro data starts at address 0x1B 
  I2C_Read_Packet_From_Sensor(&rx_temp_buffer[0],8);
  
  //get the raw data
  sen_data.gyro_x_raw = (int16_t) (((((int16_t)rx_temp_buffer[2]) << 8) | rx_temp_buffer[3])/Gyro_Gain_X);    // X axis 
  sen_data.gyro_y_raw = (int16_t) (((((int16_t)rx_temp_buffer[4]) << 8) | rx_temp_buffer[5])/Gyro_Gain_Y);    // Y axis 
  sen_data.gyro_z_raw = (int16_t) (((((int16_t)rx_temp_buffer[6]) << 8) | rx_temp_buffer[7])/Gyro_Gain_Z);    // Z axis
      
  //subtract the offset
  sen_data.gyro_x = sen_data.gyro_x_raw - sen_offset.gyro_offset[0];    // X axis 
  sen_data.gyro_y = sen_data.gyro_y_raw - sen_offset.gyro_offset[1];    // Y axis 
  sen_data.gyro_z = sen_data.gyro_z_raw - sen_offset.gyro_offset[2];    // Z axis

  //change the sign if needed
  sen_data.gyro_x *= SENSOR_SIGN[0];    // X axis 
  sen_data.gyro_y *= SENSOR_SIGN[1];    // Y axis 
  sen_data.gyro_z *= SENSOR_SIGN[2];    // Z axis 
  
}
